#include "aproximatedImplicit.h"
#include <iostream>


AproximatedImplicit::AproximatedImplicit() : IntegrationMethod()
{


}

AproximatedImplicit::AproximatedImplicit(double _deltaT, double _g, std::vector<State> _s0,
                                         std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                                         bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                                         : IntegrationMethod(_deltaT, _g, _s0, _p, _s, _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection)
{
}

AproximatedImplicit::~AproximatedImplicit()
{


}
void AproximatedImplicit::calculate(bool, unsigned int _fabricPosition, double _fraction){
    //guardando dados
    State s = currentState.at(_fabricPosition);
    double t = s.getT();
    unsigned int dof = s.getDOF();
    Matrix reference = s.getReferenceMatrix();
    std::vector< std::pair<bool, Object *> > collision = s.getCollision();


    currentDeltaT = deltaT / _fraction;

    Vector *resultanteForce = populateResultantForces(currentState.at(_fabricPosition), _fabricPosition, KANG2000);

    std::vector<Particle *> particlesI = particles.at(_fabricPosition);
    std::vector<double> stiffness = stiffnessResult(particlesI, _fabricPosition);

    //calculando
    for(unsigned int j=0; j<particlesI.size(); j++)
    {

            if((reference[j][0] == -1) && (reference[j][1] == -1) && (reference[j][2] == -1)) continue;


            double mass = particlesI.at(j)->getMass();
            double kd = particlesI.at(j)->getKd();


            Vector3D currentForce3D, currentPosition, currentVelocity;
            int place;

            currentState.at(_fabricPosition).getParticlePositionVelocity(j, particlesI.at(j)->getPosition(), particlesI.at(j)->getVelocity(), &currentPosition, &currentVelocity);


            for(unsigned int k=0; k<3; k++)
            {

                place = reference[j][k];

                if (place != -1)
                {

                    currentForce3D[k] = (*resultanteForce)[place];

                }

            }


            implicitCalculation(&s, mass, kd, *resultanteForce, stiffness, currentForce3D, currentPosition, currentVelocity, _fabricPosition, j);


        }
           //atualizando
    previousState.at(_fabricPosition) = currentState.at(_fabricPosition);
    currentState.at(_fabricPosition) = s;
    currentState.at(_fabricPosition).setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);
    delete resultanteForce;
}

std::vector<double> AproximatedImplicit::stiffnessResult(const std::vector<Particle *> &_particlesI, unsigned int _i)
{
    std::vector<Spring *> springsI = springs.at(_i);
    unsigned int size = springsI.size();
    unsigned int sizeParticle = _particlesI.size();
    std::vector<double> result(sizeParticle, 0);

    for(unsigned int j=0; j<_particlesI.size(); j++)
    {
        double rigidity = 0.0;

        for(unsigned int i = 0; i < size; i++)
        {
            Spring *s = NULL;

            s = springsI.at(i);

            if(!(s->getParticleBegin() == (int)j || s->getParticleEnd() == (int)j))
            {
                continue;
            }
            else
            {
                rigidity = rigidity + s->getKs();
            }
        }

        result[j] = rigidity;

    }


    return result;

}

Vector3D AproximatedImplicit::neighbourForceCalulate(const Matrix &_reference, Vector _resultanteForce, const std::vector<double> &_stiffness, unsigned int _i, unsigned _j)
{

    Vector3D result;
    std::vector<Particle *> particlesI = particles.at(_i);
    std::vector<Spring *> springsI = springs.at(_i);


    unsigned int size = springsI.size();

    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;

        s = springsI.at(i);

        int particle;
        Vector3D resultForce, dividend, neighbourForce;
        double mass, springStiffness, stiffness, divider;

        if(s->getParticleBegin() == (int)_j)
        {
           particle = s->getParticleEnd();
           mass = particlesI.at(particle)->getMass();
           springStiffness = s->getKs();
           stiffness = _stiffness[particle];

           divider = mass + ((currentDeltaT*currentDeltaT)*stiffness);

           for(unsigned int k=0; k<3; k++)
           {
                int place = _reference[particle][k];

                if (place != -1)
                {
                    resultForce[k] = (_resultanteForce)[place];

                }

            }
            dividend = resultForce * (springStiffness*currentDeltaT);
            neighbourForce = dividend * (1.0/divider);
            result = result + neighbourForce;
        }
        else if(s->getParticleEnd() == (int)_j)
        {
           particle = s->getParticleBegin();
           mass = particlesI.at(particle)->getMass();
           springStiffness = s->getKs();
           stiffness = _stiffness[particle];

           divider = mass + ((currentDeltaT*currentDeltaT)*stiffness);

           for(unsigned int k=0; k<3; k++)
           {
                int place = _reference[particle][k];

                if (place != -1)
                {
                    resultForce[k] = (_resultanteForce)[place];

                }

            }
            dividend = resultForce * (springStiffness*currentDeltaT);
            neighbourForce = dividend * (1.0/divider);
            result = result + neighbourForce;
        }
    }

    return result;

}

void AproximatedImplicit::implicitCalculation(State *_state, double _mass, double _kd,  Vector _resultanteForce, const std::vector<double> &_stiffness, const Vector3D &_resultantForce, const Vector3D &_currentPosition, const Vector3D &_currentVelocity, unsigned int _i, unsigned int _j)
{

    Vector3D offsetVelocity, neighbourForce;

    Matrix reference = _state->getReferenceMatrix();

    double stiffness = _stiffness[_j];

    neighbourForce = neighbourForceCalulate(reference, _resultanteForce, _stiffness, _i, _j);

    offsetVelocity = ((_resultantForce * currentDeltaT) + (neighbourForce * (currentDeltaT * currentDeltaT)))*(1.0/(_mass + (currentDeltaT *currentDeltaT * stiffness)));

    double normVelocity = _currentVelocity.length();

    Vector3D offsetDamping = ((_currentVelocity + offsetVelocity)*( 1.0 - (1.0 / ( 1 + (_kd * normVelocity)) ) ))*(-1.0);

    Vector3D velocity = _currentVelocity + offsetVelocity + offsetDamping;

    Vector3D position = _currentPosition + velocity * (currentDeltaT);


    int place;
    for(unsigned int k=0; k<3; k++)
    {
        place = reference[_j][k];

        if (place != -1)
        {
            _state->getPosition()[place] = position[k];
            _state->getVelocity()[place] = velocity[k];
        }
    }

}



